#include <ros/ros.h>
#include <base_controller/base_controller_stm32.h>

namespace base_controller{
BaseController::BaseController():serial_parser_(nullptr){
    ros::NodeHandle pravite_nh("~");
    pravite_nh.param("port", serial_port_, std::string("/dev/wheeltec_controller")); 
    pravite_nh.param("baud", serial_baud_, 115200);
    pravite_nh.param("base_frame", base_frame_, string("base_link"));
    pravite_nh.param("odom_frame", odom_frame_, string("odom"));
    pravite_nh.param("imu_frame", imu_frame_, string("imu_link"));
    pravite_nh.param("odom_tf", odom_tf_, true);

    odom_pub_ = nh_.advertise<nav_msgs::Odometry>("odom", 50);  
    imu_pub_ = nh_.advertise<sensor_msgs::Imu>("imu", 20); 
    voltage_pub_ = nh_.advertise<std_msgs::Float32>("PowerVoltage", 10);
    cmdvel_sub_ = nh_.subscribe("cmd_vel", 100, &BaseController::cmdvel_cb, this);
    serial_parser_ = new SerialParser(serial_port_,serial_baud_);

    OdomReset();
    ControlCore();
}

BaseController::~BaseController(){
    serial_parser_->SetMotorSpeed(0.0f,0.0f,0.0f);
    serial_parser_->SerialClose();
    if(serial_parser_ != nullptr) 
      delete serial_parser_;
}

// 速度回调函数
void BaseController::cmdvel_cb(const geometry_msgs::Twist::ConstPtr& vel_msg){
  if(serial_parser_ != nullptr)
    speed_time_last_ = ros::Time::now();
    serial_parser_->SetMotorSpeed(vel_msg->linear.x, vel_msg->linear.y, vel_msg->angular.z);
}

// 里程计重置
void BaseController::OdomReset(){
  robot_x_ = 0.0f;
  robot_y_ = 0.0f;
  robot_yaw_ = 0.0f;
}

/*
  发布里程计
*/
void BaseController::Publish_Odom(){
  geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(robot_yaw_); //转换为四元数

  // 里程计构造
  nav_msgs::Odometry odom_msg;
  odom_msg.header.stamp = ros::Time::now(); 
  odom_msg.header.frame_id = odom_frame_; 
  odom_msg.child_frame_id = base_frame_;
  odom_msg.pose.pose.position.x = robot_x_; 
  odom_msg.pose.pose.position.y = robot_y_;
  odom_msg.pose.pose.position.z = 0.0f;
  odom_msg.pose.pose.orientation = odom_quat; 
  odom_msg.twist.twist.linear.x =  serial_parser_->Robot_Vel.X; 
  odom_msg.twist.twist.linear.y =  serial_parser_->Robot_Vel.Y; 
  odom_msg.twist.twist.angular.z = serial_parser_->Robot_Vel.Z; 
  //这个矩阵有两种，分别在机器人静止和运动的时候使用。扩展卡尔曼滤波官方提供的2个矩阵，用于robot_pose_ekf功能包
  if(serial_parser_->Robot_Vel.X == 0 && serial_parser_->Robot_Vel.Y == 0 && serial_parser_->Robot_Vel.Z == 0) //如果velocity是零，说明编码器的误差会比较小，认为编码器数据更可靠
    memcpy(&odom_msg.pose.covariance, odom_pose_covariance2, sizeof(odom_pose_covariance2)),
    memcpy(&odom_msg.twist.covariance, odom_twist_covariance2, sizeof(odom_twist_covariance2));
  else  //如果小车velocity非零，考虑到运动中编码器可能带来的滑动误差，认为imu的数据更可靠
    memcpy(&odom_msg.pose.covariance, odom_pose_covariance, sizeof(odom_pose_covariance)),
    memcpy(&odom_msg.twist.covariance, odom_twist_covariance, sizeof(odom_twist_covariance));       
  odom_pub_.publish(odom_msg); 

  if(odom_tf_){
    // 提供坐标变换
    static tf2_ros::TransformBroadcaster br_;
    geometry_msgs::TransformStamped transformStamped;
    transformStamped.header.stamp = ros::Time::now();
    transformStamped.header.frame_id = odom_frame_;
    transformStamped.child_frame_id = base_frame_;
    transformStamped.transform.translation.x = robot_x_;
    transformStamped.transform.translation.y = robot_y_;
    transformStamped.transform.translation.z = 0.0f;
    tf2::Quaternion q_tf;
    // tf2::fromMsg(odom_quat, q_tf);
    q_tf.setRPY(0, 0, robot_yaw_);
    transformStamped.transform.rotation.x = q_tf.x();
    transformStamped.transform.rotation.y = q_tf.y();
    transformStamped.transform.rotation.z = q_tf.z();
    transformStamped.transform.rotation.w = q_tf.w();
    br_.sendTransform(transformStamped);
  }

}

//发布电源电压话题
void BaseController::Publish_Voltage()
{
    std_msgs::Float32 voltage_msgs; //Define the data type of the power supply voltage publishing topic //定义电源电压发布话题的数据类型
    static float Count_Voltage_Pub=0;
    if(Count_Voltage_Pub++>10)
      {
        Count_Voltage_Pub=0;  
        voltage_msgs.data = serial_parser_->Power_voltage; //The power supply voltage is obtained //电源供电的电压获取
        voltage_pub_.publish(voltage_msgs); //Post the power supply voltage topic unit: V, volt //发布电源电压话题单位：V、伏特
      }
}


/**************************************
Function: 平方根倒数 求四元数用到
***************************************/
float InvSqrt(float number)
{
  volatile long i;
    volatile float x, y;
    volatile const float f = 1.5F;
    x = number * 0.5F;
    y = number;
    i = * (( long * ) &y);
    i = 0x5f375a86 - ( i >> 1 );
    y = * (( float * ) &i);
    y = y * ( f - ( x * y * y ) );

  return y;
}

/**************************************
Function: 四元数解算
***************************************/
volatile float twoKp = 1.0f;     // 2 * proportional gain (Kp)
volatile float twoKi = 0.0f;     // 2 * integral gain (Ki)
volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;          // quaternion of sensor frame relative to auxiliary frame
volatile float integralFBx = 0.0f,  integralFBy = 0.0f, integralFBz = 0.0f; // integral error terms scaled by Ki
geometry_msgs::Quaternion Quaternion_Solution(float gx, float gy, float gz, float ax, float ay, float az)
{
  geometry_msgs::Quaternion msg;
  float recipNorm;
  float halfvx, halfvy, halfvz;
  float halfex, halfey, halfez;
  float qa, qb, qc;
  // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
  if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
    // 首先把加速度计采集到的值(三维向量)转化为单位向量，即向量除以模
    recipNorm = InvSqrt(ax * ax + ay * ay + az * az);
    ax *= recipNorm;
    ay *= recipNorm;
    az *= recipNorm;      
    // 把四元数换算成方向余弦中的第三行的三个元素
    halfvx = q1 * q3 - q0 * q2;
    halfvy = q0 * q1 + q2 * q3;
    halfvz = q0 * q0 - 0.5f + q3 * q3;
    //误差是估计的重力方向和测量的重力方向的交叉乘积之和
    halfex = (ay * halfvz - az * halfvy);
    halfey = (az * halfvx - ax * halfvz);
    halfez = (ax * halfvy - ay * halfvx);
    // 计算并应用积分反馈（如果启用）
    if(twoKi > 0.0f) {
      integralFBx += twoKi * halfex * (1.0f / SAMPLING_FREQ);  // integral error scaled by Ki
      integralFBy += twoKi * halfey * (1.0f / SAMPLING_FREQ);
      integralFBz += twoKi * halfez * (1.0f / SAMPLING_FREQ);
      gx += integralFBx;        // apply integral feedback
      gy += integralFBy;
      gz += integralFBz;
    }
    else {
      integralFBx = 0.0f;       // prevent integral windup
      integralFBy = 0.0f;
      integralFBz = 0.0f;
    }
    // Apply proportional feedback
    gx += twoKp * halfex;
    gy += twoKp * halfey;
    gz += twoKp * halfez;
  }
  // Integrate rate of change of quaternion
  gx *= (0.5f * (1.0f / SAMPLING_FREQ));   // pre-multiply common factors
  gy *= (0.5f * (1.0f / SAMPLING_FREQ));
  gz *= (0.5f * (1.0f / SAMPLING_FREQ));
  qa = q0;
  qb = q1;
  qc = q2;
  q0 += (-qb * gx - qc * gy - q3 * gz);
  q1 += (qa * gx + qc * gz - q3 * gy);
  q2 += (qa * gy - qb * gz + q3 * gx);
  q3 += (qa * gz + qb * gy - qc * gx); 
  // Normalise quaternion
  recipNorm = InvSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
  q0 *= recipNorm;
  q1 *= recipNorm;
  q2 *= recipNorm;
  q3 *= recipNorm;
  msg.w = q0;
  msg.x = q1;
  msg.y = q2;
  msg.z = q3;
  return msg;
}

//发布IMU话题 
void BaseController::Publish_Imu(){

  sensor_msgs::Imu imu_msg;
  imu_msg.header.stamp = ros::Time::now(); 
  imu_msg.header.frame_id = imu_frame_;
  //线性加速度单位转化，和STM32的IMU初始化的时候的量程有关,这里量程±2g=19.6m/s^2
  imu_msg.linear_acceleration.x = serial_parser_->Imu_Data.accele_x_data / ACCEl_RATIO;
  imu_msg.linear_acceleration.y = serial_parser_->Imu_Data.accele_y_data / ACCEl_RATIO;
  imu_msg.linear_acceleration.z = serial_parser_->Imu_Data.accele_z_data / ACCEl_RATIO;

  //陀螺仪单位转化，和STM32的IMU初始化的时候的量程有关，这里IMU的陀螺仪的量程是±500°/s
  //因为机器人一般Z轴速度不快，降低量程可以提高精度
  imu_msg.angular_velocity.x =  serial_parser_->Imu_Data.gyros_x_data * GYROSCOPE_RATIO;
  imu_msg.angular_velocity.y =  serial_parser_->Imu_Data.gyros_y_data * GYROSCOPE_RATIO;
  imu_msg.angular_velocity.z =  serial_parser_->Imu_Data.gyros_z_data * GYROSCOPE_RATIO;
  imu_msg.angular_velocity_covariance[0] = 1e6; //三轴角速度协方差矩阵
  imu_msg.angular_velocity_covariance[4] = 1e6;
  imu_msg.angular_velocity_covariance[8] = 1e-6;

  // 通过IMU绕三轴角速度与三轴加速度计算三轴姿态
  imu_msg.orientation = Quaternion_Solution(imu_msg.angular_velocity.x, 
                                            imu_msg.angular_velocity.y, 
                                            imu_msg.angular_velocity.z,
                                            imu_msg.linear_acceleration.x, 
                                            imu_msg.linear_acceleration.y, 
                                            imu_msg.linear_acceleration.z);
  imu_msg.orientation_covariance[0] = 1e6;    //三轴姿态协方差矩阵
  imu_msg.orientation_covariance[4] = 1e6;
  imu_msg.orientation_covariance[8] = 1e-6;
  imu_pub_.publish(imu_msg); 

  // 使用IMU的角度作为yaw
  // tf2::Quaternion imu_quat(
  //   imu_msg.orientation.x,
  //   imu_msg.orientation.y,
  //   imu_msg.orientation.z,
  //   imu_msg.orientation.w);
  // double roll, pitch;
  // tf2::Matrix3x3 m(imu_quat);
  // m.getRPY(roll, pitch, robot_yaw_);
}

// 控制核心
void BaseController::ControlCore(){
  time_last_ = ros::Time::now();
  speed_time_last_ = ros::Time::now();
  while(!ros::isShuttingDown()){
    // if(serial_parser_ == nullptr) continue;
    Sampling_Time = (time_now_ - time_last_).toSec(); 
    // ROS_INFO("Sampling_Time: %0.5f",Sampling_Time);
    time_now_ = ros::Time::now();
    if(time_now_ > speed_time_last_ + ros::Duration(0.5)){    //速度指令停止发送，机器立即停止
      if(serial_parser_ != nullptr)
          serial_parser_->SetMotorSpeed(0.0,0.0,0.0);      
    }
    if(serial_parser_->GetSensorData()) {
      //ROS_INFO("Get_Sensor_Data_New....");
      robot_x_+= (serial_parser_->Robot_Vel.X * cos(robot_yaw_) - serial_parser_->Robot_Vel.Y * sin(robot_yaw_)) * Sampling_Time;   //计算X方向的位移，单位：m
      robot_y_ += (serial_parser_->Robot_Vel.X * sin(robot_yaw_) + serial_parser_->Robot_Vel.Y * cos(robot_yaw_)) * Sampling_Time;  //计算Y方向的位移，单位：m
      robot_yaw_ += serial_parser_->Robot_Vel.Z * Sampling_Time; //绕Z轴的角位移，单位：rad 

      Publish_Odom();       //发布里程计话题
      Publish_Imu();        //发布IMU话题    
      Publish_Voltage();    //发布电源电压话题

      time_last_ = time_now_; 
    }
    ros::spinOnce();  
  }
}

}

int main(int argc, char **argv )
{
    ros::init(argc, argv, "base_controller_stm32");
    ROS_INFO("base_controller_stm32 initilization......");
    ros::NodeHandle nh;
    base_controller::BaseController base_controller_;
    ros::spin();
    return 0;
}



/*
xxty@xxty-NUC8i5BEH:~/ROBOT/trobot_ws$ rosmsg info sensor_msgs/Imu
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
geometry_msgs/Quaternion orientation
  float64 x
  float64 y
  float64 z
  float64 w
float64[9] orientation_covariance
geometry_msgs/Vector3 angular_velocity
  float64 x
  float64 y
  float64 z
float64[9] angular_velocity_covariance
geometry_msgs/Vector3 linear_acceleration
  float64 x
  float64 y
  float64 z
float64[9] linear_acceleration_covariance
*/